//-------------------------------------------------------------
#include "A.h"
//-------------------------------------------------------------
A::A(int* piLevel, int iLevelWidth, int iLevelHeight):
m_iLevelWidth(0),
m_iLevelHeight(0),
m_kLevel(NULL)
{
	changeLevel(piLevel, iLevelWidth, iLevelHeight);
}
//-------------------------------------------------------------
/*
A::A(int iLevel[], int iLevelWidth, int iLevelHeight):
m_iLevelWidth(0),
m_iLevelHeight(0),
m_kLevel(NULL)
{
	changeLevel(iLevel, iLevelWidth, iLevelHeight);
}
*/
//-------------------------------------------------------------
A::~A()
{
	if (m_kLevel)
	{
		delete[] m_kLevel;
		m_kLevel = NULL;
	}
}
//-------------------------------------------------------------
bool A::calculatePath(int iOriginCol, int iOriginRow, int iDestinationCol, int iDestinationRow, vector<int> &rkPath, int& iDistance)
{
	m_kNodosAbiertos.clear();
	m_kNodosCerrados.clear();

	int iOriginIndex = iOriginRow * m_iLevelWidth + iOriginCol;
	int iDestinIndex = iDestinationRow * m_iLevelWidth + iDestinationCol;

	resetInfo();

	int iActualIndex = iOriginIndex;
	
	m_kNodosAbiertos.push_back(&m_kLevel[iOriginIndex]);
	while (iActualIndex != iDestinIndex)
	{
		if (m_kNodosAbiertos.empty())
		{
			return false;
		}

		iActualIndex = searchLowestCost();
		removeFromOpenList(iActualIndex);
		m_kNodosCerrados.push_back(&m_kLevel[iActualIndex]);
		addAdyToOpenList(iActualIndex, iOriginIndex, iDestinIndex);
	}
	
#ifdef XYPATH
	constructXYPath(iDestinIndex, iDestinationCol, iDestinationRow, rkPath, iDistance);
#else
	constructPath(iDestinIndex, piPath, riPathLenght, rkPath, iDistance);
#endif
	return true;
}
//-------------------------------------------------------------
void A::addAdyToOpenList(int iIndex, int iOriginIndex, int iDestinIndex)
{
	// para cada nodo adyacente al actual
	for (size_t i = 0; i < m_kLevel[iIndex].kAdyacents.size(); i++)
	{
		// indice del nodo adyacente actual
		int iAdyIndex = m_kLevel[iIndex].kAdyacents[i]->iRow * m_iLevelWidth
			+ m_kLevel[iIndex].kAdyacents[i]->iCol;

		if (!(isOnClosedList(iAdyIndex)))
		{
			// si el costo no es negativo (impasable)
			if (m_kLevel[iIndex].kAdyacents[i]->iCost >= 0)
			{
				// me fijo si ya esta en la open list
				bool bFound = isOnOpenList(iAdyIndex);

				if (bFound)
				{
					//calculo el costo con el nodo actual como nuevo padre
					int iNewCost = calculateCostFromOrigin
						(iAdyIndex, iOriginIndex, m_kLevel[iIndex].iDistanceFromOrigin);

					// si es mayor al actual no cambio nada
					if (iNewCost > m_kLevel[iIndex].kAdyacents[i]->iDistanceFromOrigin)
					{	
						continue;
					}
				}
				// Seteo el padre
				m_kLevel[iIndex].kAdyacents[i]->kParent = &m_kLevel[iIndex];

				// seteo el costo desde el origen
				m_kLevel[iIndex].kAdyacents[i]->iDistanceFromOrigin = 
					calculateCostFromOrigin
					(iAdyIndex, iOriginIndex, m_kLevel[iIndex].iDistanceFromOrigin);

				// seteo la manhattan
				m_kLevel[iIndex].kAdyacents[i]->iManhattan = 
					calculateManhattan(iAdyIndex, iDestinIndex);

				// seteo costo total
				m_kLevel[iIndex].kAdyacents[i]->iTotalCost = 
					m_kLevel[iIndex].kAdyacents[i]->iDistanceFromOrigin + 
					m_kLevel[iIndex].kAdyacents[i]->iManhattan; 

				// si ya estaba lo remuevo
				if (bFound)
					removeFromOpenList(iAdyIndex);

				// lo agrego a los nodos abiertos
				m_kNodosAbiertos.push_back(m_kLevel[iIndex].kAdyacents[i]);
			}
		}
	}
}
//-------------------------------------------------------------
int A::calculateManhattan(int iIndex, int iDestinIndex)
{
	return (abs(m_kLevel[iDestinIndex].iCol - m_kLevel[iIndex].iCol) + 
		abs(m_kLevel[iDestinIndex].iRow - m_kLevel[iIndex].iRow)) * 10; 
}
//-------------------------------------------------------------
int A::calculateCostFromOrigin(int iIndex, int iOriginIndex, int iParentCost)
{
	int iBasicCost = 0;
	if ( (iOriginIndex + 1 == iIndex) || (iOriginIndex - 1 == iIndex) ||
		(iOriginIndex + m_iLevelWidth == iIndex) || (iOriginIndex - m_iLevelWidth == iIndex) )
		iBasicCost = 10;
	else
		iBasicCost = 14;

	return iParentCost + m_kLevel[iIndex].iCost + iBasicCost;
}
//-------------------------------------------------------------
bool A::isOnOpenList(int iIndex)
{
	for(vector<Node*>::iterator it = m_kNodosAbiertos.begin(); it < m_kNodosAbiertos.end(); it++)
	{
		if ( ( (*it)->iRow * m_iLevelWidth + (*it)->iCol ) == iIndex )
		{
			return true;
		}
	}

	return false;
}
//-------------------------------------------------------------
bool A::isOnClosedList(int iIndex)
{
	for(vector<Node*>::iterator it = m_kNodosCerrados.begin(); it < m_kNodosCerrados.end(); it++)
	{
		if ( ( (*it)->iRow * m_iLevelWidth + (*it)->iCol ) == iIndex )
		{
			return true;
		}
	}

	return false;
}
//-------------------------------------------------------------
void A::removeFromOpenList(int iIndex)
{
	for(vector<Node*>::iterator it = m_kNodosAbiertos.begin(); it < m_kNodosAbiertos.end(); it++)
	{
		if ( ( (*it)->iRow * m_iLevelWidth + (*it)->iCol ) == iIndex )
		{
			m_kNodosAbiertos.erase(it);
			return;
		}
	}
}
//-------------------------------------------------------------
int A::searchLowestCost()
{
	int iLowestCostIndex = m_kNodosAbiertos[0]->iRow * m_iLevelWidth + m_kNodosAbiertos[0]->iCol;
	int iLowestCost = m_kNodosAbiertos[0]->iTotalCost;
	for(vector<Node*>::iterator it = m_kNodosAbiertos.begin() + 1; it < m_kNodosAbiertos.end(); it++)
	{
		if ( (*it)->iTotalCost < iLowestCost )
		{
			iLowestCostIndex = (*it)->iRow * m_iLevelWidth + (*it)->iCol;
			iLowestCost = (*it)->iTotalCost;
		}
	}
	return iLowestCostIndex;
}
//-------------------------------------------------------------
void A::constructPath(int iDestinIndex, int* &piPath, size_t& riPathLenght, vector<int> &rkPath, int& iDistance)
{
	iDistance = m_kLevel[iDestinIndex].iDistanceFromOrigin;
	int iIndex = iDestinIndex;
	vector<int> kPath;
	kPath.push_back(iDestinIndex);
	rkPath.push_back(iDestinIndex);
	while (m_kLevel[iIndex].kParent)
	{
		iIndex = m_kLevel[iIndex].kParent->iRow * m_iLevelWidth + m_kLevel[iIndex].kParent->iCol;
		kPath.push_back(iIndex);
		rkPath.push_back(iIndex);
	}

	if (piPath)
		delete[] piPath;

	piPath = new int[kPath.size()];
	piPath = &(kPath[0]);

	riPathLenght = kPath.size();

}
//-------------------------------------------------------------
void A::constructXYPath(int iDestinIndex, int iDestinationCol, int iDestinationRow, vector<int> &rkPath, int& iDistance)
{
	iDistance = m_kLevel[iDestinIndex].iDistanceFromOrigin;
	int iIndex = iDestinIndex;
	vector<int> kPath;
	rkPath.push_back(iDestinationRow);
	rkPath.push_back(iDestinationCol);
	while (m_kLevel[iIndex].kParent)
	{
		rkPath.push_back(m_kLevel[iIndex].kParent->iRow);
		rkPath.push_back(m_kLevel[iIndex].kParent->iCol);
		iIndex = m_kLevel[iIndex].kParent->iRow * m_iLevelWidth + m_kLevel[iIndex].kParent->iCol;
	}
}
//-------------------------------------------------------------
void A::resetInfo()
{
	for (int i = 0; i < m_iLevelHeight; i++)
	{
		for (int j = 0; j < m_iLevelWidth; j++)
		{
			m_kLevel[i * m_iLevelWidth + j].iDistanceFromOrigin = 0;
			m_kLevel[i * m_iLevelWidth + j].iManhattan = 0;
			m_kLevel[i * m_iLevelWidth + j].iTotalCost = 0;
			m_kLevel[i * m_iLevelWidth + j].kParent = NULL;
		}
	}
}
//-------------------------------------------------------------
void A::changeLevel(int* piLevel, int iLevelWidth, int iLevelHeight)
{
	m_iLevelWidth = iLevelWidth;
	m_iLevelHeight = iLevelHeight;

	if (m_kLevel)
	{
		delete[] m_kLevel;
		m_kLevel = NULL;
	}
	m_kLevel = new Node[m_iLevelWidth * m_iLevelHeight];

	for (int i = 0; i < m_iLevelHeight; i++)
	{
		for (int j = 0; j < m_iLevelWidth; j++)
		{
			m_kLevel[i * m_iLevelWidth + j].iCol = j;
			m_kLevel[i * m_iLevelWidth + j].iRow = i;
			m_kLevel[i * m_iLevelWidth + j].iCost = piLevel[i * m_iLevelWidth + j];
			m_kLevel[i * m_iLevelWidth + j].iDistanceFromOrigin = 0;
			m_kLevel[i * m_iLevelWidth + j].iManhattan = 0;
			m_kLevel[i * m_iLevelWidth + j].iTotalCost = 0;
			m_kLevel[i * m_iLevelWidth + j].kParent = NULL;
			calcuteAdyacents(&m_kLevel[i * m_iLevelWidth + j]);
		}
	}
}
//-------------------------------------------------------------
/*
void A::changeLevel(int iLevel[], int iLevelWidth, int iLevelHeight)
{
	m_iLevelWidth = iLevelWidth;
	m_iLevelHeight = iLevelHeight;

	if (m_kLevel)
	{
		delete[] m_kLevel;
		m_kLevel = NULL;
	}
	m_kLevel = new Node[m_iLevelWidth * m_iLevelHeight];

	for (int i = 0; i < m_iLevelHeight; i++)
	{
		for (int j = 0; j < m_iLevelWidth; j++)
		{
			m_kLevel[i * m_iLevelWidth + j].iCol = j;
			m_kLevel[i * m_iLevelWidth + j].iRow = i;
			m_kLevel[i * m_iLevelWidth + j].iCost = iLevel[i * m_iLevelWidth + j];
			m_kLevel[i * m_iLevelWidth + j].iDistanceFromOrigin = 0;
			m_kLevel[i * m_iLevelWidth + j].iManhattan = 0;
			m_kLevel[i * m_iLevelWidth + j].iTotalCost = 0;
			m_kLevel[i * m_iLevelWidth + j].kParent = NULL;
			calcuteAdyacents(&m_kLevel[i * m_iLevelWidth + j]);
		}
	}
}
*/
//-------------------------------------------------------------
void A::calcuteAdyacents(Node* kNode)
{
	int iIndex = kNode->iRow * m_iLevelWidth + kNode->iCol;

	int iAdyIndex = iIndex + 1;
	if ((iIndex + 1) % m_iLevelWidth != 0)
		if (iAdyIndex >= 0 && iAdyIndex < m_iLevelWidth * m_iLevelHeight)
			kNode->kAdyacents.push_back(&m_kLevel[iAdyIndex]);

	iAdyIndex = iIndex - 1;
	if (iIndex % m_iLevelWidth != 0)
		if (iAdyIndex >= 0 && iAdyIndex < m_iLevelWidth * m_iLevelHeight)
			kNode->kAdyacents.push_back(&m_kLevel[iAdyIndex]);

	iAdyIndex = iIndex + m_iLevelWidth;
	if (iAdyIndex >= 0 && iAdyIndex < m_iLevelWidth * m_iLevelHeight)
		kNode->kAdyacents.push_back(&m_kLevel[iAdyIndex]);

	iAdyIndex = iIndex - m_iLevelWidth;
	if (iAdyIndex >= 0 && iAdyIndex < m_iLevelWidth * m_iLevelHeight)
		kNode->kAdyacents.push_back(&m_kLevel[iAdyIndex]);

	iAdyIndex = iIndex + (m_iLevelWidth + 1);
	if ((iIndex + 1) % m_iLevelWidth != 0)
		if (iAdyIndex >= 0 && iAdyIndex < m_iLevelWidth * m_iLevelHeight)
			kNode->kAdyacents.push_back(&m_kLevel[iAdyIndex]);

	iAdyIndex = iIndex - (m_iLevelWidth + 1);
	if (iIndex % m_iLevelWidth != 0)
		if (iAdyIndex >= 0 && iAdyIndex < m_iLevelWidth * m_iLevelHeight)
			kNode->kAdyacents.push_back(&m_kLevel[iAdyIndex]);

	iAdyIndex = iIndex + (m_iLevelWidth - 1);
	if (iIndex % m_iLevelWidth != 0)
		if (iAdyIndex >= 0 && iAdyIndex < m_iLevelWidth * m_iLevelHeight)
			kNode->kAdyacents.push_back(&m_kLevel[iAdyIndex]);

	iAdyIndex = iIndex - (m_iLevelWidth - 1);
	if ((iIndex + 1) % m_iLevelWidth != 0)
		if (iAdyIndex >= 0 && iAdyIndex < m_iLevelWidth * m_iLevelHeight)
			kNode->kAdyacents.push_back(&m_kLevel[iAdyIndex]);
}
//-------------------------------------------------------------

/*
TEORIA!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!
 function A*(start,goal)
     closedset := the empty set                 % The set of nodes already evaluated.
     openset := set containing the initial node % The set of tentative nodes to be evaluated.
     g_score[start] := 0                        % Distance from start along optimal path.
     h_score[start] := heuristic_estimate_of_distance(start, goal)
     f_score[start] := h_score[start]           % Estimated total distance from start to goal through y.
     while openset is not empty
         x := the node in openset having the lowest f_score[] value
         if x = goal
             return reconstruct_path(came_from,goal)
         remove x from openset
         add x to closedset
         foreach y in neighbor_nodes(x)
             if y in closedset
                 continue
             tentative_g_score := g_score[x] + dist_between(x,y)
             tentative_is_better := false
             if y not in openset
                 add y to openset
                 h_score[y] := heuristic_estimate_of_distance(y, goal)
                 tentative_is_better := true
             elseif tentative_g_score < g_score[y]
                 tentative_is_better := true
             if tentative_is_better = true
                 came_from[y] := x
                 g_score[y] := tentative_g_score
                 f_score[y] := g_score[y] + h_score[y]
     return failure
 
 function reconstruct_path(came_from,current_node)
     if came_from[current_node] is set
         p = reconstruct_path(came_from,came_from[current_node])
         return (p + current_node)
     else
         return the empty path
*/
